﻿////////////////////////////////////////////////////////////////////////////
//                                                                        //
//                       Copyright (c) 2003-2006                          //
//                              Wang Hua                                  //
//                                                                        //
////////////////////////////////////////////////////////////////////////////

// AsAttitudeParam.cpp
//
//////////////////////////////////////////////////////////////////////


#include "AsAttitudeParam.h"
#include "AsCoordinate.h"


using namespace std;




//////////////////////////////////////////////////////////////////////
// CEuler Construction/Destruction
//////////////////////////////////////////////////////////////////////
CEuler::CEuler() : m_Angle1(0.0), m_Angle2(0.0), m_Angle3(0.0)
{
}

CEuler::CEuler(double ang1, double ang2, double ang3)
			 : m_Angle1(ang1), m_Angle2(ang2), m_Angle3(ang3)
{
}

CEuler::~CEuler()
{
}


///***********************************************************************
/// 根据旋转角度和旋转轴计算坐标转移矩阵
/// @Author	Wang Hua
/// @Date	2004.11.24
/// @Input
/// @Param	seqFlag	旋转sequence
/// @Output
/// @Param	mtx		返回的转移矩阵
///***********************************************************************
void CEuler::ToMtx (int seqFlag, CMatrix<double>& mtx) const
{
	assert(seqFlag==123 || seqFlag==132 || seqFlag==121 || seqFlag==131 
		|| seqFlag==213 || seqFlag==231 || seqFlag==212 || seqFlag==232
		|| seqFlag==321 || seqFlag==312 || seqFlag==313 || seqFlag==323);

	int m;
	CMatrix<double> mat(3,3);

	m       = seqFlag/100;
	AsEulerToMtx(m_Angle1, m, mtx);

	seqFlag = seqFlag-m*100;
	m       = seqFlag/10;
	AsEulerToMtx(m_Angle2, m, mat);
	mtx     = mat*mtx;

	seqFlag = seqFlag-m*10;
	AsEulerToMtx(m_Angle3, seqFlag, mat);
	mtx     = mat*mtx;
}


///***********************************************************************
/// 根据旋转角度和旋转轴计算quaternion
/// @Author	Wang Hua
/// @Date	2004.11.24
/// @Input
/// @Param	seqFlag	旋转sequence
/// @Output
/// @Param	quat	四元数
///***********************************************************************
void CEuler::ToQuat (int seqFlag, CQuaternion& quat) const
{
	quat.FromEuler(*this, seqFlag);
}



//////////////////////////////////////////////////////////////////////
// CQuaternion Construction/Destruction
//////////////////////////////////////////////////////////////////////

CQuaternion::CQuaternion()
			 : m_Qs(1.0), m_Qx(0.0), m_Qy(0.0), m_Qz(0.0)
{
}

CQuaternion::CQuaternion(double e,double x,double y,double z)
			 : m_Qs(e), m_Qx(x), m_Qy(y), m_Qz(z)
{
}

CQuaternion::~CQuaternion()
{

}


const CQuaternion CQuaternion::operator +() const
{
	return *this;
}


const CQuaternion CQuaternion::operator -() const
{
	CQuaternion q;
	q.m_Qs = -m_Qs;
	q.m_Qx = -m_Qx;
	q.m_Qy = -m_Qy;
	q.m_Qz = -m_Qz;
	return q;
}


const CQuaternion& CQuaternion::operator +=(const CQuaternion& q)
{
	m_Qs += q.m_Qs;
	m_Qx += q.m_Qx;
	m_Qy += q.m_Qy;
	m_Qz += q.m_Qz;
	return *this;
}


const CQuaternion CQuaternion::operator +(const CQuaternion& q) const
{
	CQuaternion qu = *this;
	qu += q;
	return qu;
}


const CQuaternion& CQuaternion::operator -=(const CQuaternion& q)
{
	m_Qs -= q.m_Qs;
	m_Qx -= q.m_Qx;
	m_Qy -= q.m_Qy;
	m_Qz -= q.m_Qz;
	return *this;
}


const CQuaternion CQuaternion::operator -(const CQuaternion& q) const
{
	CQuaternion qu = *this;
	qu -= q;
	return qu;
}


const CQuaternion& CQuaternion::operator *=(const CQuaternion& q)
{
	CQuaternion q0 = *this;
	m_Qs = q0.m_Qs*q.m_Qs-q0.m_Qx*q.m_Qx-q0.m_Qy*q.m_Qy-q0.m_Qz*q.m_Qz;
	m_Qx = q0.m_Qs*q.m_Qx+q0.m_Qx*q.m_Qs+q0.m_Qy*q.m_Qz-q0.m_Qz*q.m_Qy;
	m_Qy = q0.m_Qs*q.m_Qy-q0.m_Qx*q.m_Qz+q0.m_Qy*q.m_Qs+q0.m_Qz*q.m_Qx;
	m_Qz = q0.m_Qs*q.m_Qz+q0.m_Qx*q.m_Qy-q0.m_Qy*q.m_Qx+q0.m_Qz*q.m_Qs;
	return *this;
}


const CQuaternion CQuaternion::operator *(const CQuaternion& q) const
{
	CQuaternion qu = *this;
	qu *= q;
	return qu;
}


const CQuaternion& CQuaternion::operator*=(double d)
{
	m_Qs *= d;
	m_Qx *= d;
	m_Qy *= d;
	m_Qz *= d;
	return *this;
}


const CQuaternion CQuaternion::operator *(double d) const
{
	CQuaternion q = *this;
	q *= d;
	return q;
}

	
const CQuaternion operator *(double d, const CQuaternion& q)
{
	return q*d;
}


const CQuaternion& CQuaternion::operator/=(double d)
{
	m_Qs /= d;
	m_Qx /= d;
	m_Qy /= d;
	m_Qz /= d;
	return *this;
}


const CQuaternion CQuaternion::operator /(double d) const
{
	CQuaternion q = *this;
	q /= d;
	return q;
}

// Equalities, inequalities.
bool CQuaternion::operator==(const CQuaternion& q) const
{
    return (m_Qs == q.m_Qs) && (m_Qx == q.m_Qx) && (m_Qy == q.m_Qy) && (m_Qz == q.m_Qz);
}

bool CQuaternion::operator!=(const CQuaternion& q) const
{
    return !(*this == q); 
}


//得到四元数的共轭四元数
const CQuaternion CQuaternion::Conjugate() const
{
	CQuaternion q;
	q.m_Qs =  m_Qs;
	q.m_Qx = -m_Qx;
	q.m_Qy = -m_Qy;
	q.m_Qz = -m_Qz;
	return q;
}

//得到四元数的逆元数
const CQuaternion CQuaternion::Invert() const
{
	return Conjugate()/Norm();
}


//得到四元数的范数
double CQuaternion::Norm() const
{
	return m_Qs*m_Qs+m_Qx*m_Qx+m_Qy*m_Qy+m_Qz*m_Qz;
}


void CQuaternion::Unit()
{
	assert(Norm()!=0);

	*this /= sqrt(m_Qs*m_Qs+m_Qx*m_Qx+m_Qy*m_Qy+m_Qz*m_Qz);
}


///***********************************************************************
/// 根据旋转角度和旋转坐标轴计算四元数
/// @Author	Wang Hua
/// @Date	2004.3
/// @Input
/// @Param	angle	旋转角度
/// @Param	axFlag	旋转轴(1,2,3-X,Y,Z)
/// @Output	
///***********************************************************************
void CQuaternion::FromEuler(double angle, int axFlag)
{
	assert(axFlag>=1 && axFlag<=3);

	double a = angle*0.5;
	m_Qs = cos(a);
	switch(axFlag)
	{
	case 1:
		m_Qx = sin(a);
		m_Qy = 0.0;
		m_Qz = 0.0;
		break;
	case 2:
		m_Qx = 0.0;
		m_Qy = sin(a);
		m_Qz = 0.0;
		break;
	case 3:
		m_Qx = 0.0;
		m_Qy = 0.0;
		m_Qz = sin(a);
		break;
	}
}


///***********************************************************************
/// 根据Euler角和旋转坐标轴计算四元数
/// @Author	Wang Hua
/// @Date	2004.11.24
/// @Input
/// @Param	euler	旋转Euler角
/// @Param	seqFlag	旋转轴(1,2,3-X,Y,Z)
/// @Output	
///***********************************************************************
void CQuaternion::FromEuler(const CEuler& angle, int seqFlag)
{
	assert(seqFlag==123 || seqFlag==132 || seqFlag==121 || seqFlag==131 
		|| seqFlag==213 || seqFlag==231 || seqFlag==212 || seqFlag==232
		|| seqFlag==321 || seqFlag==312 || seqFlag==313 || seqFlag==323);

	int m;
	CQuaternion q1;

	m       = seqFlag/100;
	FromEuler(angle.m_Angle1, m);

	seqFlag = seqFlag-m*100;
	m       = seqFlag/10;
	q1.FromEuler(angle.m_Angle2, m);
	*this  *= q1;

	seqFlag = seqFlag-m*10;
	q1.FromEuler(angle.m_Angle3, seqFlag);
	*this  *= q1;

	Unit();
}


///***********************************************************************
/// 根据旋转角度和旋转坐标轴计算四元数
/// @Author	Wang Hua
/// @Date	2004.10.22
/// @Input
/// @Param	angle	旋转角
/// @Param	seqFlag	旋转轴(1,2,3-X,Y,Z)
///***********************************************************************
void CQuaternion::FromEuler(const CVector<double>& angle, const CVector<int>& seqFlag)
{
	assert(angle.GetSize()==seqFlag.GetSize());

	CQuaternion q1;
	int n = angle.GetSize();
	if (n>0)
	{
		FromEuler(angle[0],seqFlag[0]);
	}
	for (int i=1; i<n; i++)
	{
		q1.FromEuler(angle[i],seqFlag[i]);
		*this *= q1;
	}

	Unit();
}


///***********************************************************************
/// 根据旋转角度和旋转轴计算四元数
/// @Author	Wang Hua
/// @Date	2004.10.12
/// @Input
/// @Param	axis	旋转轴矢量
/// @Param	angle	旋转角
///***********************************************************************
void CQuaternion::FromAxAng(const CCoord& axis, double angle)
{
	CCoord ax(axis);
	ax.Unit();
	double a = angle*0.5;
	double b = sin(a);
	m_Qs = cos(a);
	m_Qx = axis[0]*b;
	m_Qy = axis[1]*b;
	m_Qz = axis[2]*b;
}


///***********************************************************************
/// 根据四元数计算旋转角度和旋转轴
/// @Author	Wang Hua
/// @Date	2004.10.26
/// @Output
/// @Param	angle	旋转角度
/// @Param	axFlag	旋转轴
///***********************************************************************
void CQuaternion::ToAxAng(CCoord& axis, double& angle) const
{
	angle = 2.0*acos(m_Qs);
	double b = sin(angle*0.5);
	if (b!=0)
	{
		axis[0] = m_Qx/b;
		axis[1] = m_Qy/b;
		axis[2] = m_Qz/b;
	}
	else
	{
		axis[0] = 1.0;
		axis[1] = 0.0;
		axis[2] = 0.0;
	}
}


///***********************************************************************
/// 方向余弦矩阵转换成四元数
/// @Author	Wang Hua
/// @Date	2004.4
/// @Input
/// @Param	mtx		旋转矩阵
///***********************************************************************
void CQuaternion::FromMtx(const CMatrix<double>& m)
{
	double w = 1.0+m[0][0]+m[1][1]+m[2][2];
	m_Qs = sqrt(w)*0.5;
	double tempdbl = 4.0*m_Qs;
	m_Qx = (m[1][2]-m[2][1])/tempdbl;
	m_Qy = (m[2][0]-m[0][2])/tempdbl;
	m_Qz = (m[0][1]-m[1][0])/tempdbl;

	Unit();
}


///***********************************************************************
/// 四元数转换成方向余弦矩阵
/// @Author	Wang Hua
/// @Date	2004.4
/// @Output
/// @Param	axis	旋转轴矢量
/// @Param	angle	旋转角
///***********************************************************************
void CQuaternion::ToMtx(CMatrix<double>& m) const
{
	assert(m.GetSizeCol()==3 && m.GetSizeRow()==3);

	double xy = m_Qx*m_Qy;
	double yz = m_Qy*m_Qz;
	double zx = m_Qz*m_Qx;
	double sx = m_Qs*m_Qx;
	double sy = m_Qs*m_Qy;
	double sz = m_Qs*m_Qz;
	double xx = m_Qx*m_Qx;
	double yy = m_Qy*m_Qy;
	double zz = m_Qz*m_Qz;

	m[0][0] = 1.0-2.0*(yy+zz);	m[0][1] = 2.0*(xy+sz);		m[0][2] = 2.0*(zx-sy);
	m[1][0] = 2.0*(xy-sz);		m[1][1] = 1.0-2.0*(xx+zz);	m[1][2] = 2.0*(yz+sx);
	m[2][0] = 2.0*(zx+sy);		m[2][1] = 2.0*(yz-sx);		m[2][2] = 1.0-2.0*(xx+yy);

	for (int i=0; i<3; i++)
	{
		for (int j=0; j<3; j++)
		{
			if (m[i][j] > 1.0)
				m[i][j] = 1;
			else if (m[i][j] < -1.0)
				m[i][j] = -1;
		}
	}
}


///***********************************************************************
/// 根据旋转角度和旋转轴计算坐标转移矩阵
/// @Author	Wang Hua
/// @Date	2004.3
/// @Input
/// @Param	angle	旋转角度
/// @Param	axFlag	旋转轴
/// @Output
/// @Param	mtx		返回的转移矩阵
///***********************************************************************
void  AsEulerToMtx(double angle, int axFlag, CMatrix<double>& mtx)
{
	assert(axFlag<=3 && axFlag>0);

	double c = cos(angle);
	double s = sin(angle);
	switch(axFlag)
	{
	case 1:
		mtx[0][0]=1.0;	mtx[0][1]=0.0;	mtx[0][2]=0.0;
		mtx[1][0]=0.0;	mtx[1][1]=c;	mtx[1][2]=s;
		mtx[2][0]=0.0;	mtx[2][1]=-s;	mtx[2][2]=c;
		break;
	case 2:
		mtx[0][0]=c;	mtx[0][1]=0.0;	mtx[0][2]=-s;
		mtx[1][0]=0.0;	mtx[1][1]=1.0;	mtx[1][2]=0.0;
		mtx[2][0]=s;	mtx[2][1]=0.0;	mtx[2][2]=c;
		break;
	case 3:
		mtx[0][0]=c;	mtx[0][1]=s;	mtx[0][2]=0.0;
		mtx[1][0]=-s;	mtx[1][1]=c;	mtx[1][2]=0.0;
		mtx[2][0]=0.0;	mtx[2][1]=0.0;	mtx[2][2]=1.0;
		break;
	}
}


///***********************************************************************
/// 根据旋转角度和旋转轴计算坐标转移矩阵
/// @Author	Wang Hua
/// @Date	2004.11.24
/// @Input
/// @Param	euler	Euler角
/// @Param	seqFlag	旋转sequence
/// @Output	
/// @Param	mtx		返回的转移矩阵
///***********************************************************************
void  AsEulerToMtx (const CEuler& euler, int seqFlag,
								CMatrix<double>& mtx)
{
	assert(seqFlag==123 || seqFlag==132 || seqFlag==121 || seqFlag==131 
		|| seqFlag==213 || seqFlag==231 || seqFlag==212 || seqFlag==232
		|| seqFlag==321 || seqFlag==312 || seqFlag==313 || seqFlag==323);

	euler.ToMtx(seqFlag, mtx);
}


///***********************************************************************
/// 根据旋转角度和旋转轴计算坐标转移矩阵
/// @Author	Wang Hua
/// @Date	2004.9.7
/// @Input
/// @Param	angle	旋转角度(弧度)
/// @Param	seqFlag	旋转轴标志:1=x,2=y,3=z
/// @Param	n		输入的角的个数
/// @Output	
/// @Param	mtx		转移矩阵
///***********************************************************************
void  AsEulerToMtx(const double angle[],
			const int seqFlag[], int n, CMatrix<double>& mtx)
{
    if (n<=0)
    {
        mtx.MakeUnit();
        return;
    }

	CMatrix<double> m(3,3);
	AsEulerToMtx(angle[0],seqFlag[0],mtx);
	for(int i=1; i<n; i++)
	{
		AsEulerToMtx(angle[i],seqFlag[i],m);
		mtx = m*mtx;
	}
}


///***********************************************************************
/// 根据旋转角度和旋转轴计算坐标转移矩阵
/// @Author	Wang Hua
/// @Date	2004.9.7
/// @Input
/// @Param	angle	旋转角度(弧度)
/// @Param	seqFlag	旋转轴标志:1=x,2=y,3=z
/// @Output
/// @Param	mtx		转移矩阵
///***********************************************************************
void  AsEulerToMtx(const CVector<double>& angle,
			const CVector<int>& seqFlag, CMatrix<double>& mtx)
{
	assert(angle.GetSize()==seqFlag.GetSize());
    AsEulerToMtx(angle.Data(), seqFlag.Data(), angle.GetSize(), mtx);
}


///***********************************************************************
/// 根据Euler角计算quaternion
/// @Author	Wang Hua
/// @Date	2004.11.24
/// @Input
/// @Param	euler	Euler角
/// @Param	seqFlag	旋转sequence
/// @Output
/// @Param	quat	四元数
///***********************************************************************
void  AsEulerToQuat (const CEuler& euler, int seqFlag,
			CQuaternion& quat)
{
	quat.FromEuler(euler, seqFlag);
}


///***********************************************************************
/// The conversion from Euler axis and angle to matrix.
/// @Author	Wang Hua
/// @Date	2004.10.13
/// @Input
/// @Param	axis	旋转轴矢量
/// @Param	angle	旋转角
/// @Output
/// @Param	mtx		转移矩阵
///***********************************************************************
void  AsAxAngToMtx(const CCoord& axis, double angle,
			CMatrix<double>& mtx)
{
	CCoord ax(axis);
	ax.Unit();
	double cc    = cos(angle);
	double ss    = sin(angle);
	double sigma = 1.0-cc;
	mtx[0][0] = ax[0]*ax[0]*sigma+cc;
	mtx[0][1] = ax[0]*ax[1]*sigma+ax[2]*ss;
	mtx[0][2] = ax[0]*ax[2]*sigma-ax[1]*ss;
	mtx[1][0] = ax[0]*ax[1]*sigma-ax[2]*ss;
	mtx[1][1] = ax[1]*ax[1]*sigma+cc;
	mtx[1][2] = ax[1]*ax[2]*sigma+ax[0]*ss;
	mtx[2][0] = ax[0]*ax[2]*sigma+ax[1]*ss;
	mtx[2][1] = ax[1]*ax[2]*sigma-ax[0]*ss;
	mtx[2][2] = ax[2]*ax[2]*sigma+cc;
}


///***********************************************************************
/// The conversion from axis and angle to quaternion.
/// @Author	Wang Hua
/// @Date	2004.11.24
/// @Input
/// @Param	axis	旋转轴矢量
/// @Param	angle	旋转角
/// @Output
/// @Param	quat	四元数
///***********************************************************************
void  AsAxAngToQuat(const CCoord& axis, double angle,
			CQuaternion& quat)
{
	quat.FromAxAng(axis, angle);
}


///***********************************************************************
/// The conversion from matrix to quaternion.
/// @Author	Wang Hua
/// @Date	2004.11.24
/// @Input
/// @Param	mtx		转移矩阵
/// @Output
/// @Param	quat	四元数
///***********************************************************************
void  AsMtxToQuat (const CMatrix<double>& mtx, CQuaternion& quat)
{
	quat.FromMtx(mtx);
}


///***********************************************************************
/// The conversion from quaternion to axis and angle.
/// @Author	Wang Hua
/// @Date	2004.11.24
/// @Input
/// @Param	quat	四元数
/// @Output
/// @Param	axis	旋转轴矢量
/// @Param	angle	旋转角[0, pi]
///***********************************************************************
void  AsQuatToAxAng(const CQuaternion& quat, CCoord& axis,
			double& angle)
{
	quat.ToAxAng(axis, angle);
}


///***********************************************************************
/// The conversion from quaternion to matrix.
/// @Author	Wang Hua
/// @Date	2004.11.24
/// @Input
/// @Param	quat	四元数
/// @Output
/// @Param	mtx		转移矩阵
///***********************************************************************
void  AsQuatToMtx(const CQuaternion& quat, CMatrix<double>& mtx)
{
	quat.ToMtx(mtx);
}


///***********************************************************************
/// 四元数转为313转序Euler角
/// @Author	Fan Yunbao
/// @Date	2025.3
/// @Input
/// @Param	quat
/// @Output
/// @Param	euler	欧拉角(rad)
///***********************************************************************
void AsQuatToEuler313(const CQuaternion& quat, CEuler& euler)
{
	// 检查四元数是否归一化
	double norm = quat.m_Qs * quat.m_Qs + quat.m_Qx * quat.m_Qx + quat.m_Qy * quat.m_Qy + quat.m_Qz * quat.m_Qz;
	if (std::abs(norm - 1.0) > std::numeric_limits<double>::epsilon()) {
		std::cerr << "Warning: The quaternion is not normalized!" << std::endl;
		return;
	}

	// 提取单位化后的四元数分量
	double q0 = quat.m_Qs;  // 四元数的实部
	double q1 = quat.m_Qx;  // 四元数的 x 分量
	double q2 = quat.m_Qy;  // 四元数的 y 分量
	double q3 = quat.m_Qz;  // 四元数的 z 分量

	// 计算 313 转序的欧拉角
	double sin_theta = 2.0 * (q1 * q3 - q0 * q2);
	// 处理 asin 输入超出 [-1, 1] 范围的情况
	if (sin_theta > 1.0) {
		sin_theta = 1.0;
	}
	else if (sin_theta < -1.0) {
		sin_theta = -1.0;
	}

	euler.m_Angle2 = asin(sin_theta);  // theta (绕 X 轴)

	// 处理绕X轴旋转+-pi/2的情形，此时直接将phi赋值为零，以防止多解。
	if (fabs(sin_theta) >= 1.0 - std::numeric_limits<double>::epsilon()) {
		euler.m_Angle1 = 0.0;  // phi (绕 Z 轴)
		euler.m_Angle3 = 2.0 * atan2(q1, q0);  // psi (绕 Z 轴)
	}
	else {
		euler.m_Angle1 = atan2(2.0 * (q2 * q3 + q0 * q1), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);  // phi (绕 Z 轴)
		euler.m_Angle3 = atan2(2.0 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3);  // psi (绕 Z 轴)
	}
}


///***********************************************************************
/// 将四元数转为3-1-2转序的欧拉角
/// @Author	田磊
/// @Date	2025.3.05
/// @Input
/// @Param	quat	四元数
/// @Output
/// @Param	euler	3-1-2转序欧拉角
///***********************************************************************
void AsQuatToEuler312(const CQuaternion& quat,CEuler& euler)
{
	// 检查四元数是否归一化
	double norm = quat.m_Qs * quat.m_Qs + quat.m_Qx * quat.m_Qx + quat.m_Qy * quat.m_Qy + quat.m_Qz * quat.m_Qz;
	if (std::abs(norm - 1.0) > std::numeric_limits<double>::epsilon()) {
		std::cerr << "Warning: The quaternion is not normalized!" << std::endl;
		return;
	}
	// 提取单位化后的四元数分量
	double q0 = quat.m_Qs;  // 四元数的实部
	double q1 = quat.m_Qx;  // 四元数的 x 分量
	double q2 = quat.m_Qy;  // 四元数的 y 分量
	double q3 = quat.m_Qz;  // 四元数的 z 分量
	double sin_fi = 2.0 * (q2 * q3 + q0 * q1);
	// 处理 asin 输入超出 [-1, 1] 范围的情况
	if (sin_fi > 1.0) {
		sin_fi = 1.0;
	}
	else if (sin_fi < -1.0) {
		sin_fi = -1.0;
	}
	// 计算 312 转序的欧拉角
	euler.m_Angle2 = asin(sin_fi);
	// 处理绕x轴旋转+-pi/2的情形，此时直接将theta赋值为零，以防止多解。
	if (fabs(2.0 * (q2 * q3 + q0 * q1)) >= 1.0 - std::numeric_limits<double>::epsilon()) {
		euler.m_Angle3 = 0.0;  // theta (绕 Y 轴)
		euler.m_Angle1 = 2.0 * atan2(q3, q0);  // psi (绕 Z 轴)
	}
	else {
		euler.m_Angle1 = atan2(2.0 * (-q1 * q2 + q0 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3);  // psi (绕 Z 轴)
		euler.m_Angle3 = atan2(2.0 * (-q1 * q3 + q0 * q2), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);  // theta (绕 Y 轴)
	}
}



///***********************************************************************
/// The conversion from matrix to axis and angle.
/// @Author	Xiao Yao
/// @Date	2023.4.4
/// @Input
/// @Param	mtx	    坐标转换矩阵
/// @Output
/// @Param	axis	旋转轴矢量
/// @Param	angle	旋转角[0, pi]
///***********************************************************************
void AsMtxToAxAng(const CMatrix<double>& mtx,
    CCoord& axis, double& angle)
{

    CMatrix<double> mm = mtx;
    //************** 判断是否为正交矩阵 ******************
    double p1 = mm[0][0] * mm[0][1] + mm[1][0] * mm[1][1] + mm[2][0] * mm[2][1];
    double p2 = mm[0][0] * mm[0][2] + mm[1][0] * mm[1][2] + mm[2][0] * mm[2][2];
    double p3 = mm[0][2] * mm[0][1] + mm[1][2] * mm[1][1] + mm[2][2] * mm[2][1];
    double p4 = mm[0][0] * mm[0][0] + mm[1][0] * mm[1][0] + mm[2][0] * mm[2][0];
    double p5 = mm[0][1] * mm[0][1] + mm[1][1] * mm[1][1] + mm[2][1] * mm[2][1];
    double p6 = mm[0][2] * mm[0][2] + mm[1][2] * mm[1][2] + mm[2][2] * mm[2][2];
    if (!(fabs(p1)<1e-9 && fabs(p2)<1e-9 && fabs(p3)<1e-9 && 
        fabs(p4-1)<1e-9 && fabs(p5 - 1)<1e-9 && fabs(p6 - 1)<1e-9) ){
        printf("warning! not Orthogonal Matrix\n");
        angle = 0;
        axis[0] = 0;
        axis[1] = 0;
        axis[2] = 0;
        return;
    }

    // 矩阵的迹
    double T = mm[0][0] + mm[1][1] + mm[2][2];

    if (fabs(T - 3) > 1e-9 && fabs(T + 1) > 1e-9) {
        double cc = 0.5 * (T - 1);   // cos(angle)
        double ss = sin(acos(cc));   // sin(angle)
        angle = atan2(ss, cc);       // (0, pi)
        axis[0] = 0.5 * (mm[1][2] - mm[2][1]) / ss;
        axis[1] = 0.5 * (mm[2][0] - mm[0][2]) / ss;
        axis[2] = 0.5 * (mm[0][1] - mm[1][0]) / ss;
    }
    else if (fabs(T + 1) < 1e-9) {
        // 转轴有两个解，指向相差180°,且角度不影响, 本程序仅输出一个
        // sin(angle) = 0， angle=pi+2k*pi,k为整数
        angle = AsCPI;

        double a12 = mm[0][1] / 2.0;
        double a23 = mm[1][2] / 2.0;
        double a31 = mm[2][0] / 2.0;
        double aa1 = sqrt((1.0 + mm[0][0]) / 2.0);
        double aa2 = sqrt((1.0 + mm[1][1]) / 2.0);
        double aa3 = sqrt((1.0 + mm[2][2]) / 2.0);
        if (a12 > 0 && a23 > 0 && a31 > 0) {
            // a1 a2 a3同号
            axis[0] = aa1;
            axis[1] = aa2;
            axis[2] = aa3;
        }
        else if (a12 > 0) {
            // a1 a2 同号
            axis[0] = aa1;
            axis[1] = aa2;
            axis[2] = -aa3;
        }
        else if (a23 > 0) {
            // a2 a3 同号
            axis[0] = aa1;
            axis[1] = -aa2;
            axis[2] = -aa3;
        }
        else {
            // a1 a3同号
            axis[0] = aa1;
            axis[1] = -aa2;
            axis[2] = aa3;
        }
    }
    else {
        // 转轴不确定
        angle = 0;
        axis[0] = 1;
        axis[1] = 0;
        axis[2] = 0;
    }
}


///***********************************************************************
///将四元数转化成欧拉角_123转序
/// @Author	Liu Tianqing
/// @Date	2023.04.06
/// @Input
/// @Param	q		标准化四元数
/// @Output
/// @Param	euler		欧拉角(rad)
///***********************************************************************
bool AsQuatToEuler123(const CQuaternion& q, CEuler& euler)
{
    //设置默认参数
    double epsilon = 1e-6;		//精确度
    euler = { 0, 0, 0 };
    //q0, q1, q2, q3分别对应q.m_Qs, q.m_Qx, q.m_Qy, q.m_Qz
    double q0q0 = q.m_Qs*q.m_Qs;
    double q1q1 = q.m_Qx*q.m_Qx;
    double q2q2 = q.m_Qy*q.m_Qy;
    double q3q3 = q.m_Qz*q.m_Qz;
    double q_norm2 = q0q0 + q1q1 + q2q2 + q3q3;
    if (fabs(q_norm2 - 1) > epsilon)			//判断输入的四元数是否标准化
    {
        cout << "请输入标准化四元数！！！" << endl;
        return false;
    }
    else if (fabs(q0q0 - 1) < epsilon) {}	//没有旋转时，直接输出
    else
    {
        double q0q1 = q.m_Qs*q.m_Qx;
        double q0q2 = q.m_Qs*q.m_Qy;
        double q0q3 = q.m_Qs*q.m_Qz;
        double q1q2 = q.m_Qx*q.m_Qy;
        double q1q3 = q.m_Qx*q.m_Qz;
        double q2q3 = q.m_Qy*q.m_Qz;

        double a02 = q0q0 - q2q2;
        double a13 = q1q1 - q3q3;
        double sy = 2 * (q1q3 + q0q2);   //俯仰角 angle.y 的正弦值

                                         //判断是否出现奇异，当出现奇异时，令 Angle3或Angle1 为 0，确保程序可以继续运行，这里默认 Angle3为0
        if (sy - 1 >= 0)		//判断俯仰角为pi/2
        {
            euler.m_Angle2 = AsCHalfPI;
            euler.m_Angle1 = 2*atan(q.m_Qx / q.m_Qy);
        }
        else if (sy + 1 <= 0)   //判断俯仰角为-pi/2
        {
            euler.m_Angle2 = -AsCHalfPI;
            euler.m_Angle1 = -2*atan(q.m_Qx / q.m_Qy);
        }
        else
        {
            euler.m_Angle2 = asin(2 * (q1q3 + q0q2));
            euler.m_Angle1 = atan2(2 * (q0q1 - q2q3), a02 - a13);
            euler.m_Angle3 = atan2(2 * (q0q3 - q1q2), a02 + a13);
        }
    }
    return true;
}

/***********************************************************************
将四元数转化成欧拉角_132转序
@Author	Xiong yanci
@Date	2025.02.27
@Input
@Param	q		标准化四元数
@Output
@Param	euler		欧拉角(rad)
***********************************************************************/
bool AsQuatToEuler132(const CQuaternion& q, CEuler& euler)
{
    ////设置默认参数
    double epsilon = 1e-6;		//精确度
    euler = { 0, 0, 0 };

    double q0, q1, q2, q3;
    double q0q1, q0q2, q0q3, q1q3, q1q2, q2q3;
    double q0q0, q1q1, q2q2, q3q3;
    double q_norm1,q_norm2;
    double norm_sq;

    q0 = q.m_Qs;
    q1 = q.m_Qx;
    q2 = q.m_Qy;
    q3 = q.m_Qz;

    q0q1 = q0 * q1;
    q0q2 = q0 * q2;
    q0q3 = q0 * q3;
    q1q2 = q1 * q2;
    q1q3 = q1 * q3;
    q2q3 = q2 * q3;
    q0q0 = q0 * q0;
    q1q1 = q1 * q1;
    q2q2 = q2 * q2;
    q3q3 = q3 * q3;

    q_norm1 = q0q0 + q1q1 - q2q2 - q3q3;
    q_norm2 = q0q0 - q1q1 + q2q2 - q3q3;
    norm_sq = q0q0 + q1q1 + q2q2 + q3q3;
    double t = 2 * q0q3 - 2 * q1q2;

    if(fabs(norm_sq - 1) > epsilon)//判断输入的四元数是否标准化
    {
        cout << "请输入标准化四元数！！！" << endl; 
        return false;//自动归一化，输出000值
    }
    else
    {   
        //	//判断是否出现奇异，当出现奇异时，令roll = Angle1 为 0，确保程序可以继续运行
        //yaw
        if (t > 1)
        {
            euler.m_Angle3 = AsCHalfPI;
            euler.m_Angle1 = 2 * atan(q1/q0);
        }

        else if (t < -1)
        {
            euler.m_Angle3 = -AsCHalfPI;
            euler.m_Angle1 = 2 * atan(q1 / q0);
        }

        else
        {
            //XZY四元数转欧拉角
            euler.m_Angle2 = atan2((2 * q0q2 + 2 * q1q3),q_norm1);  //roll
            euler.m_Angle1 = atan2((2 * q0q1 + 2 * q2q3),q_norm2);  //pitch
            euler.m_Angle3 = asin(t);               //yaw
        }

    }
    return true;
}

///***********************************************************************
///将四元数转化成欧拉角_213转序
/// @Author	Kong Lingyue
/// @Date	2025.03.10
/// @Input
/// @Param	q		标准化四元数
/// @Output
/// @Param	euler		欧拉角(rad)
///***********************************************************************
bool AsQuatToEuler213(const CQuaternion& q, CEuler& euler)
{
    //设置默认参数
    double epsilon = 1e-6;		//精确度
    euler = { 0, 0, 0 };
    //q0, q1, q2, q3分别对应q.m_Qs, q.m_Qx, q.m_Qy, q.m_Qz
    double q0q0 = q.m_Qs*q.m_Qs;
    double q1q1 = q.m_Qx*q.m_Qx;
    double q2q2 = q.m_Qy*q.m_Qy;
    double q3q3 = q.m_Qz*q.m_Qz;
    double q_norm2 = q0q0 + q1q1 + q2q2 + q3q3;
    if (fabs(q_norm2 - 1.0) > epsilon)			//判断输入的四元数是否标准化
    {
        cout << "请输入标准化四元数！！！" << endl;
        return false;
    }

    else if (fabs(q0q0 - 1) < epsilon) {}	//没有旋转时，直接输出
    else
    {
        double q0q1 = q.m_Qs*q.m_Qx;
        double q0q2 = q.m_Qs*q.m_Qy;
        double q0q3 = q.m_Qs*q.m_Qz;
        double q1q2 = q.m_Qx*q.m_Qy;
        double q1q3 = q.m_Qx*q.m_Qz;
        double q2q3 = q.m_Qy*q.m_Qz;
        cout << q0q1 << endl;
        double s = -2 * (q2q3 - q0q1);   //正弦值
                                         //判断是否出现奇异，当出现奇异时，令 Angle3或Angle1 为 0，确保程序可以继续运行，这里默认 Angle3为0
        if (s > 1 - epsilon)		//判断为pi/2
        {
            euler.m_Angle2 = AsCHalfPI;
            euler.m_Angle1 = 2 * atan(q.m_Qy / q.m_Qs);
            euler.m_Angle3 = 0.0;
        }
        else if (s < - 1 + epsilon)   //判断为-pi/2
        {
            euler.m_Angle2 = -AsCHalfPI;
            euler.m_Angle1 = 2 * atan(q.m_Qy / q.m_Qs) ;
            euler.m_Angle3 = 0.0;
        }
        else
        {
            euler.m_Angle2 = asin( s );
            euler.m_Angle1 = -atan2( -2 * (q1q3 + q0q2), 1 - 2 * (q1q1 + q2q2));
            euler.m_Angle3 = -atan2(-2 * (q1q2 + q0q3), 1 - 2 * (q1q1 + q3q3));
        }
    }
    return true;
}


///***********************************************************************
/// 方向余弦阵转321转序欧拉角
/// @Author	Li Sibei
/// @Date	2023.04.06
/// @Input
/// @Param	mtx		方向余弦阵
/// @Output
/// @Param	euler	欧拉角
///***********************************************************************
void AsMtxToEuler321(const CMatrix<double>& mtx, CEuler& euler)
{
    //异常数据处理1：输入非正交矩阵
    CMatrix<double> MTX1(mtx);
    CMatrix<double> mtx1 = MTX1.Transpose();
    CMatrix<double> mtx2 = MTX1.Inv();
    CMatrix<double> mtx3 = mtx1 - mtx2;
    for (int i = 0; i <= 2; i++)
    {
        for (int j = 0; j <= 2; j++)
        {
            if (mtx3[i][j] > 1e-15 || mtx3[i][j] < -1e-15)
            {
                printf("Error: 方向余弦阵非正交矩阵!\n");
                return;
            }
        }
    }
    //异常数据处理2：奇异
    if (mtx[0][2] == 1)
    {
        euler.m_Angle1 = -atan2(mtx[2][1], mtx[1][1]);
        euler.m_Angle2 = -asin(mtx[0][2]);
        euler.m_Angle3 = 0;
        return;
    }
    else if (mtx[0][2] == -1)
    {
        euler.m_Angle1 = atan2(mtx[2][1], mtx[1][1]);
        euler.m_Angle2 = -asin(mtx[0][2]);
        euler.m_Angle3 = 0;
        return;
    }
    //一般情况求解
    else
    {
        euler.m_Angle1 = atan2(mtx[0][1], mtx[0][0]);
        euler.m_Angle2 = -asin(mtx[0][2]);
        euler.m_Angle3 = atan2(mtx[1][2], mtx[2][2]);
    }
}


///***********************************************************************
/// 方向余弦矩阵转为313转序Euler角
/// @Author	Wang Weili
/// @Date	2023.4
/// @Input
/// @Param	mtx		坐标转移矩阵
/// @Output
/// @Param	euler	欧拉角(rad)
///***********************************************************************
void AsMtxToEuler313(const CMatrix<double>& mtx, CEuler& euler)
{
    if (mtx[2][2] < +1)
    {
        if (mtx[2][2] > -1)
        {
            euler.m_Angle1 = atan2(mtx[2][0], -mtx[2][1]);
            euler.m_Angle2 = acos(mtx[2][2]);
            euler.m_Angle3 = atan2(mtx[0][2], mtx[1][2]);
        }
        else //mtx[2][2]==-1
        {
            euler.m_Angle1 = atan2(mtx[0][1], mtx[0][0]);
            euler.m_Angle2 = AsCPI;
            euler.m_Angle3 = 0;
        }
    }
    else //mtx[2][2]==+1
    {
        euler.m_Angle1 = atan2(mtx[0][1], mtx[0][0]);
        euler.m_Angle2 = 0;
        euler.m_Angle3 = 0;
    }
}


///***********************************************************************
/// 方向余弦矩阵转为312转序Euler角
/// @Author	Zhang Xuaying
/// @Date	2023.04.01
/// @Input  
/// @Param	mtx  		方向余弦矩阵
/// @Output	
/// @Param	euler		欧拉角(rad)
///***********************************************************************
void AsMtxToEuler312(const CMatrix<double>& mtx, CEuler& euler)
{
    bool singular = mtx(1, 2) > 1 - 1e-8;//判断是否出现欧拉角奇异问题	
    if (!singular)
    {
        euler.m_Angle2 = asin(mtx(1, 2));
        //偏航
        if (mtx(1, 0) == 0)
        {
            euler.m_Angle1 = 0;
        }
        else
        {
            euler.m_Angle1 = -atan2(mtx(1, 0), mtx(1, 1));
        }

        //俯仰
        if (mtx(0, 2) == 0)
        {
            euler.m_Angle3 = 0;
        }
        else
        {
            euler.m_Angle3 = -atan2(mtx(0, 2), mtx(2, 2));
        }
    }
    else
    {
        cout << "出现欧拉角数值奇异问题 " << endl;
        euler.m_Angle1 = 0;
        euler.m_Angle2 = AsCHalfPI;
        euler.m_Angle3 = atan2(mtx(2, 0), mtx(0, 0));
    }
}


///******************************************************
/// 根据转序和坐标转移矩阵计算Euler角，转序为2-3-1
/// @author Sun Fengyan
/// @data   2023.4
/// @Version    1.0
/// @Input  
/// @Param  mtx       坐标转移矩阵
/// @Output
/// @Param  euler     返回的欧拉角(单位：rad）
///******************************************************
void  AsMtxToEuler231(const CMatrix<double>& mtx, CEuler& euler)
{
    euler.m_Angle2 = asin(mtx[0][1]); 

    if (fabs(mtx[0][1] - 1) < 1e-8 ) //euler.m_Angle2 = pi/2
    {
        euler.m_Angle1 = atan2(mtx[1][2], mtx[2][2]);
        euler.m_Angle3 = 0;
    }
    else if (fabs(mtx[0][1] + 1) < 1e-8 ) //euler.m_Angle2 = -pi/2
    {
        euler.m_Angle1 = -atan2(mtx[1][2], mtx[2][2]);
        euler.m_Angle3 = 0;
    }
    else 
    {
        euler.m_Angle1 = atan2(-mtx[0][2], mtx[0][0]);
        euler.m_Angle3 = atan2(-mtx[2][1], mtx[1][1]);

    }
}


///***********************************************************************
/// The conversion from matrix to angle123.
/// @Author	gong he
/// @Date	2023.4.4
/// @Input
/// @Param	mtx	    坐标转换矩阵
/// @Output
/// @Param	eular	旋转角[0, pi]
///***********************************************************************
void AsMtxToEuler123(const CMatrix<double>& mtx,
    CEuler& euler)
{
    CMatrix<double> mm = mtx;
    double theta = 0, psi = 0, pfi = 0;
    if (fabs(mm[2][0]) != 1) {
        if (cos(-asin(mm(2, 0)) >= 0)) {
            theta = -asin(mm(2, 0));
            psi = atan2(mm(2, 1) / cos(theta), mm(2, 2) / cos(theta));
            pfi = atan2(mm(1, 0) / cos(theta), mm(0, 0) / cos(theta));
        }
        else {
            theta = AsCPI - theta;
            psi = atan2(mm(2, 0) / cos(theta), mm(2, 2) / cos(theta));
            pfi = atan2(mm(1, 0) / cos(theta), mm(0, 0) / cos(theta));
        }
    }
    else {
        double pfi = 0;
        double delta = atan2(mm(0, 1), mm(0, 2));
        if (mm[2][0] == -1) {
            theta = AsCPI / 2;
            psi = pfi + delta;
        }
        else {
            theta = -AsCPI / 2;
            psi = -pfi + delta;
        }
    }
    euler.m_Angle1 = pfi;
    euler.m_Angle2 = theta;
    euler.m_Angle3 = psi;
}



///***********************************************************************
/// 将四元数转换为321转序的欧拉角(Z-Y-X)  
/// 遵循航天器标准定义：偏航角->俯仰角->滚转角  
/// 对应绕惯性系Z轴->新Y轴->新X轴的连续旋转  
/// @Author	王薪程（你们可爱的学长哦~） 
/// @Date	2025-03-03  
/// @Version	3.2.1  
///  
/// @Input  
/// @Param	quat	输入四元数(需标准化，不标准化也无所谓啦），顺序:标量Qs,矢量QxQyQz)  
///  
/// @Output  
/// @Param	euler	输出欧拉角结构体(单位：弧度)  
///			        m_Angle3: 绕X轴滚转角 φ ∈ [-π,π] 
///			        m_Angle2: 绕Y轴俯仰角 θ ∈ [-π/2,π/2]  
///			        m_Angle1: 绕Z轴偏航角 ψ ∈ [-π,π]   
///  
/// @Return	true: 转换成功  false: 四元数未标准化  
///  
/// @算法说明：  
/// 1. 通过四元数导出旋转矩阵关键元素，避免完整矩阵计算  
/// 2. 采用显式代数展开提升计算效率  
/// 3. 奇异位置处理逻辑与AsMtxToEuler321()保持兼容  
/// 4. 数值阈值统一采用工程标准1e-7
///***********************************************************************  
void AsQuatToEuler321(CQuaternion& quat, CEuler& euler)
{
    const double eps = 1e-7;          // 工程计算浮点误差阈值  
    euler = { 0.0, 0.0, 0.0 };         // 显式初始化输出参数  

                                       //============ 四元数标准化验证 ============  
    const double norm_sq = quat.m_Qs * quat.m_Qs +
        quat.m_Qx * quat.m_Qx +
        quat.m_Qy * quat.m_Qy +
        quat.m_Qz * quat.m_Qz;
    if (fabs(norm_sq - 1.0) > eps)
    {
        printf("[ERR]AsQuatToEuler321: 四元数未标准化! 模长平方=%.4f\n", norm_sq);
        quat.Norm();  // 强制标准化四元数
    }

    //======= 计算旋转矩阵关键元素(代数展开式) =======  
    const double q0 = quat.m_Qs;      // 标量部分  
    const double q1 = quat.m_Qx;
    const double q2 = quat.m_Qy;
    const double q3 = quat.m_Qz;

    /* 旋转矩阵R(321转序)关键元素
    [  cy*cz           cy*sz          -sy     ]
    [ sy*sx*cz-sz*cx  sy*sx*sz+cz*cx   cy*sx ]
    [ sy*cx*cz+sz*sx  sy*cx*sz-cz*sx   cy*cx ]
    */
    // 仅计算必要元素以提升性能  
    double R02 = 2 * (q1 * q3 - q0 * q2);     // -sy*cy ≈ -sin(theta)  
    double R00 = 1 - 2 * (q2 * q2 + q3 * q3); // cy*cz  
    double R01 = 2 * (q1 * q2 + q0 * q3);     // cy*sz  
    double R12 = 2 * (q2 * q3 + q0 * q1);     // cy*sx  
    double R22 = 1 - 2 * (q1 * q1 + q2 * q2); // cy*cx  

                                              //========= 俯仰角计算(保证θ∈[-π/2,π/2]) ========= 
    if (R02 > 1) R02 = 1;  // 防止数值误差导致acos溢出
    if (R02 < -1) R02 = -1;  // 防止数值误差导致acos溢出
    euler.m_Angle2 = -asin(R02);  // 对应321转序第二个旋转  

                                  //======= 万向节锁处理(θ≈±π/2时) =======  
    if (fabs(R02) > 1.0 - eps)
    {
        /* 奇异情况处理逻辑
        当θ=±π/2时，滚转角φ与偏航角ψ不可唯一确定
        约定固定ψ=0，通过矩阵元素计算φ
        与方向余弦阵转换函数AsMtxToEuler321()保持兼容
        */
        euler.m_Angle1 = 0;  // 固定偏航角  

                             // 根据R22符号确定θ的正负  
        const double sign_theta = (R02 > 0) ? -1.0 : 1.0;

        /* 计算滚转角φ
        使用R12和R22元素：
        sy*cx = R12/(2*cy)
        cy*cx = R22
        当θ→±π/2时，cy→0，改用q0和q1计算
        */
        euler.m_Angle3 = 2 * atan2(q1, q0);
    }
    else
    {
        //======= 常规情况计算 ========  
        // 偏航角ψ(z轴): atan2(cy*sz, cy*cz)  
        euler.m_Angle1 = atan2(R01, R00);

        // 滚转角φ(x轴): atan2(cy*sx, cy*cx)  
        euler.m_Angle3 = atan2(R12, R22);
    }

    //===== 角度范围修正 =====  
    // 保证ψ∈[-π,π]，φ∈[-π,π]  
    if (euler.m_Angle3 > AsCPI) euler.m_Angle3 -= 2 * AsCPI;
    if (euler.m_Angle3 < -AsCPI) euler.m_Angle3 += 2 * AsCPI;
    if (euler.m_Angle1 > AsCPI) euler.m_Angle1 -= 2 * AsCPI;
    if (euler.m_Angle1 < -AsCPI) euler.m_Angle1 += 2 * AsCPI;

    //// 测试用，转为角度制
    //euler.m_Angle3 = euler.m_Angle3 * 180 / AsCPI;
    //euler.m_Angle2 = euler.m_Angle2 * 180 / AsCPI;
    //euler.m_Angle1 = euler.m_Angle1 * 180 / AsCPI;
}

///***********************************************************************
/// 四元数323转换成Euler角
/// @Author	BXT
/// @Date	2025.3
/// @Input
/// @Param quat		四元数
/// @Output
/// @Param euler	欧拉角
///***********************************************************************
void AsQuatToEuler323(CQuaternion& quat, CEuler& euler)
{
    const double eps = 1e-8;

    // 四元数标准化验证
    const double norm_sq = quat.m_Qs * quat.m_Qs + quat.m_Qx * quat.m_Qx + quat.m_Qy * quat.m_Qy + quat.m_Qz * quat.m_Qz;
    if (fabs(norm_sq - 1.0) > eps)
    {
        printf("[ERR]AsQuatToEuler321: 四元数未标准化! 模长平方=%.4f\n", norm_sq);
        return;
    }

    // 将四元数转换为方向余弦矩阵
    CMatrix<double> m(3, 3);
    AsQuatToMtx(quat, m);

    // 提取方向余弦矩阵中的元素
    double m11 = m[0][0];
    double m12 = m[0][1];
    double m13 = m[0][2];
    double m21 = m[1][0];
    double m22 = m[1][1];
    double m23 = m[1][2];
    double m31 = m[2][0];
    double m32 = m[2][1];
    double m33 = m[2][2];

    // 计算ZYZ顺序的欧拉角
    // 参考公式：
    // 旋转矩阵m(323转序)
    // m = [cos(a)*cos(b)*cos(c)-sin(a)*sin(c), cos(a)*sin(c)+cos(b)*cos(c)*sin(a), -cos(c)*sin(b)
    //	   -cos(c)*sin(a)-cos(a)*cos(b)*sin(c), cos(a)*cos(c)-cos(b)*sin(a)*sin(c), sin(b)*sin(c)
    //	   cos(a)*sin(b), sin(a)*sin(b), cos(b)]
    // θ = acos(m33)
    // φ = atan2(m32, m31)
    // ψ = atan2(m23, -m13)

    // 计算theta并判断奇异情况,θ∈[0,π]
    double cosTheta = m33;
    euler.m_Angle2 = acos(cosTheta);

    double sinTheta = sqrt(1.0 - cosTheta*cosTheta);
    if (sinTheta > eps) 
    {
        // 非奇异情况，正常计算
        euler.m_Angle1 = atan2(m32, m31);
        euler.m_Angle3 = atan2(m23, -m13);
    }
    else 
    {
        // 奇异情况：theta接近0或π
        // theta接近0
        if (cosTheta > 0) 
        {
            euler.m_Angle1 = atan2(m12, m11);
            euler.m_Angle3 = 0.0;
        }
        // theta接近π
        else 
        {
            euler.m_Angle1 = atan2(-m21, -m11);
            euler.m_Angle3 = 0.0;
        }
    }

    // 确保m_Angle1和m_Angle3在[-π,π]
    if (euler.m_Angle3 > AsCPI) euler.m_Angle3 -= 2 * AsCPI;
    if (euler.m_Angle3 < -AsCPI) euler.m_Angle3 += 2 * AsCPI;
    if (euler.m_Angle1 > AsCPI) euler.m_Angle1 -= 2 * AsCPI;
    if (euler.m_Angle1 < -AsCPI) euler.m_Angle1 += 2 * AsCPI;

}

///***********************************************************************
/// 根据坐标转移矩阵计算132转序欧拉角
/// @Author	Wang Deyu
/// @Date	2025.03.15
/// @Input
/// @Param	mtx     输入的转移矩阵
/// @Output	
/// @Param	euler	欧拉角    rad
///***********************************************************************
void AsMtxToEuler132(const CMatrix<double>& mtx, CEuler& euler)
{
    double cbeta;
    //处理万向锁	
    if (fabs(-mtx[1][0] - 1) < 1e-12)
    {
        euler.m_Angle1 = 0;
        euler.m_Angle2 = AsCPI / 2;
        euler.m_Angle3 = atan2(-mtx[0][2], mtx[0][1]);
    }
    else if(fabs(-mtx[1][0] + 1) < 1e-12)
    {
        euler.m_Angle1 = 0;
        euler.m_Angle2 = -AsCPI / 2;
        euler.m_Angle3 = atan2(-mtx[0][2], -mtx[0][1]);
    }
    else
    {
        cbeta = sqrt(mtx[1][1] * mtx[1][1] + mtx[1][2] * mtx[1][2]);
        euler.m_Angle1 = atan2(mtx[1][2] / cbeta, mtx[1][1] / cbeta);
        euler.m_Angle2 = asin(-mtx[1][0]);
        euler.m_Angle3 = atan2(mtx[2][0] / cbeta, mtx[0][0] / cbeta);
    }
}

///***********************************************************************
/// 方向余弦矩阵转为212转序Euler角
/// @Author	Chang Xiaokuan
/// @Date	2025.2.28
/// @Input
/// @Param	mtx		坐标转移矩阵
/// @Output
/// @Param	euler	欧拉角(rad)
///***********************************************************************
void AsMtxToEuler212(const CMatrix<double>& mtx, CEuler& euler)
{	
    double epsilon = 1e-12;
    if (mtx[1][1]<(1 + epsilon) && mtx[1][1]>(-1 - epsilon))
    {
        euler.m_Angle2 = acos(mtx[1][1]);
    }
    else
    {
        cout << "error,输入方向余弦矩阵存在大于1或小于-1的元素";
    }

    if (fabs(sin(euler.m_Angle2)) < epsilon)
    {
        euler.m_Angle1 = 0;
        euler.m_Angle3 = atan2(mtx[0][2], mtx[0][0]);
    }
    else //sin(theta) != 0
    {
        euler.m_Angle1 = AsCPI-atan2(mtx[1][0], -mtx[1][2]);
        euler.m_Angle3 = AsCPI-atan2(mtx[0][1], mtx[2][1]);
    }
}

///***********************************************************************
/// 方向余弦矩阵转为121转序Euler角
/// @Author  Ma Zihui
/// @Date	20235.3
/// @Input
/// @Param	mtx		坐标转移矩阵
/// @Output
/// @Param	euler	欧拉角(rad)
///***********************************************************************
void AsMtxToEuler121(const CMatrix<double> &mtx,
    CEuler &euler)
{
    if (fabs(mtx[0][0]) != 1)
    {
        euler.m_Angle1 = atan2(mtx[0][1], -mtx[0][2]);
        euler.m_Angle2 = acos(mtx[0][0]);
        euler.m_Angle3 = atan2(mtx[1][0], mtx[2][0]);
    }
    else
    {
        euler.m_Angle1 = 0;
        double summ = atan2(-mtx[2][1], mtx[1][1]);
        if (mtx[0][0] == -1)
        {
            euler.m_Angle2 = AsCPI;
            euler.m_Angle3 = summ;
        }
        else
        {
            euler.m_Angle2 = 0;
            euler.m_Angle3 = summ;
        }
    }
}

